/*
 * File:   mcQuaternion.h
 * Author: thales
 *
 * Created on September 30, 2009, 5:33 PM
 */

#include "mcMatrix.h"

using namespace mc;

/**
 * @brief Construtor padrão, constroi uma matriz 3x3 com o valor passado
 *
 * @param Valor a ser setado em todas as posições
 */
template <class T>
matrix3<T>::matrix3( const T &t ) {
    this->_00 = t; this->_01 = t; this->_02 = t;
    this->_10 = t; this->_11 = t; this->_12 = t;
    this->_20 = t; this->_21 = t; this->_22 = t;
}


/**
 * @brief Construtor padrão, constroi uma matriz identidade
 *
 * @param Valor a ser setado em todas as posições
 */
template <class T>
matrix3<T>::matrix3() {
    this->make_identity();
}


/**
 * @brief Construtor explicito, constroi uma matriz 3x3 com os valores passados
 *
 * @param __00 - Posição 00
 * @param __01 - Posição 01
 * @param __11 - Posição 11
 * @param __12 - Posição 12
 */
template <class T>
matrix3<T>::matrix3(
    const T __00, const T __01, const T __02,
    const T __10, const T __11, const T __12,
    const T __20, const T __21, const T __22
)
: _00(__00), _01(__01), _02(__02),
  _10(__10), _11(__11), _12(__12),
  _20(__20), _21(__21), _22(__22)
{}


/**
 * @brief Construtor por cópia de vetor, constroi uma matriz 3x3 com o vetor passado
 *
 * @param m - Matriz a ser copiada
 */
template <class T>
matrix3<T>::matrix3( const T *m ) {
    this->set_data(m);
}


/**
 * @brief Construtor por cópia de matriz. Constrói a partir de uma matriz 2x2
 *
 * @param m - Matriz a ser copiada
 */
template <class T>
matrix3<T>::matrix3( const matrix2<T> &m ) {
    this->_00 = m._00; this->_01 = m._01; this->_02 = (T) 0;
    this->_10 = m._10; this->_11 = m._11; this->_12 = (T) 0;
    this->_20 = (T) 0; this->_21 = (T) 0; this->_22 = (T) 0;
}

/**
 * @brief Construtor por cópia de matriz.
 *
 * @param m - Matriz a ser copiada
 */
template <class T>
matrix3<T>::matrix3( const matrix3<T> &m ) {
    this->_00 = m._00; this->_01 = m._01; this->_02 = m._02;
    this->_10 = m._10; this->_11 = m._11; this->_12 = m._12;
    this->_20 = m._20; this->_21 = m._21; this->_22 = m._22;
}

/**
 * @brief Construtor por cópia de matriz. Constrói a partir de uma matriz 4x4
 *
 * @param m - Matriz a ser copiada
 */
template <class T>
matrix3<T>::matrix3( const matrix4<T> &m ) {
    this->_00 = m._00; this->_01 = m._01; this->_02 = m._02;
    this->_10 = m._10; this->_11 = m._11; this->_12 = m._12;
    this->_20 = m._20; this->_21 = m._21; this->_22 = m._22;
}


/**
 * @brief Acessa um elemento da matriz
 *
 * @param i - Acesso a linha
 * @param j - Acesso a coluna
 */
template <class T>
T matrix3<T>::get_element( int i, int j ) {
    assert( i >= 0 && i < this->get_row_count() && j >= 0 && j < this->get_col_count() );
    return this->_array[this->get_col_count() * j + i];
}

/**
 * @brief Seta um elemento da matriz 3x3
 *
 * @param i - Linha
 * @param j - Coluna
 * @param e - Elemento a ser setado
 */
template <class T>
void matrix3<T>::set_element( int i, int j, T e ) {
    assert( i >= 0 && i < this->get_row_count() && j >= 0 && j < this->get_col_count() );
    this->_array[this->get_col_count() * j + i] = e;
}


/**
 * @brief Seta o vetor com os dados da matriz 2x2
 *
 * @return Os dados da matriz
 */
template <class T>
void matrix3<T>::set_data( const T *array ) {
    for( register unsigned int i = 0; i < this->get_size(); i++) this->_array[i] = array[i];
}


/**
 * @brief Cria uma matriz identidade 3x3
 */
template <class T>
void matrix3<T>::make_identity() {
    this->_00 = (T) 1; this->_01 = (T) 0; this->_02 = (T) 0;
    this->_10 = (T) 0; this->_11 = (T) 1; this->_12 = (T) 0;
    this->_20 = (T) 0; this->_21 = (T) 0; this->_22 = (T) 1;
}

/**
 * @brief Operador de soma de matrizes 2x2
 *
 * @param m1
 * @param m2
 */
template <class T>
matrix3<T>& matrix3<T>::operator + ( const matrix3<T> &m ) {
    matrix3<T> *mr = new matrix3<T>( (T) 0 );
    mr->_00 = this->_00 + m._00;
    mr->_01 = this->_01 + m._01;
    mr->_02 = this->_02 + m._02;
    mr->_10 = this->_01 + m._02;
    mr->_11 = this->_11 + m._11;
    mr->_12 = this->_12 + m._12;
    mr->_20 = this->_20 + m._20;
    mr->_21 = this->_21 + m._21;
    mr->_22 = this->_22 + m._22;
    return *mr;
}


/**
 * @brief Operador de subtração de matrizes 3x3
 *
 * @param m1
 * @param m2
 */
template <class T>
matrix3<T>& matrix3<T>::operator - ( const matrix3<T> &m ) {
    matrix3<T> *mr = new matrix3<T>( (T) 0 );
    mr->_00 = this->_00 - m._00;
    mr->_01 = this->_01 - m._01;
    mr->_02 = this->_02 - m._02;
    mr->_10 = this->_01 - m._02;
    mr->_11 = this->_11 - m._11;
    mr->_12 = this->_12 - m._12;
    mr->_20 = this->_20 - m._20;
    mr->_21 = this->_21 - m._21;
    mr->_22 = this->_22 - m._22;
    return *mr;
}

/**
 * @brief Operador de multiplicação de matrizes 3x3
 *
 * @param m1
 * @param m2
 */
template <class T>
matrix3<T>& matrix3<T>::operator * ( const matrix3<T> &m ) {
    matrix3<T> *mr = new matrix3<T>( (T) 0 );
    mr->_00 = this->_00 * m->_00 + this->_01 * m->_10 + this->_02 * m->_20;
    mr->_01 = this->_00 * m->_01 + this->_01 * m->_11 + this->_02 * m->_21;
    mr->_02 = this->_00 * m->_02 + this->_01 * m->_12 + this->_02 * m->_22;
    mr->_10 = this->_10 * m->_00 + this->_11 * m->_10 + this->_12 * m->_20;
    mr->_11 = this->_10 * m->_01 + this->_11 * m->_11 + this->_12 * m->_21;
    mr->_12 = this->_10 * m->_02 + this->_11 * m->_12 + this->_12 * m->_22;
    mr->_20 = this->_20 * m->_00 + this->_21 * m->_10 + this->_22 * m->_20;
    mr->_21 = this->_20 * m->_01 + this->_21 * m->_11 + this->_22 * m->_21;
    mr->_22 = this->_20 * m->_02 + this->_21 * m->_12 + this->_22 * m->_22;
    return *mr;
}

/**
 * @brief Operador de soma de matrizes 3x3
 *
 * @param m1
 * @param m2
 */
template <class T>
matrix3<T>& matrix3<T>::operator += ( const matrix3<T> &m ) {
    this->_00 += m._00;
    this->_01 += m._01;
    this->_02 += m._02;
    this->_01 += m._02;
    this->_11 += m._11;
    this->_12 += m._12;
    this->_20 += m._20;
    this->_21 += m._21;
    this->_22 += m._22;
    return *this;
}


/**
 * @brief Operador de subtração de matrizes 3x3
 *
 * @param m1
 * @param m2
 */
template <class T>
matrix3<T>& matrix3<T>::operator -= ( const matrix3<T> &m ) {
    this->_00 -= m._00;
    this->_01 -= m._01;
    this->_02 -= m._02;
    this->_01 -= m._02;
    this->_11 -= m._11;
    this->_12 -= m._12;
    this->_20 -= m._20;
    this->_21 -= m._21;
    this->_22 -= m._22;
    return *this;
}


/**
 * @brief Operador de multiplicação de matrizes 3x3
 *
 * @param m1
 * @param m2
 */
template <class T>
matrix3<T>& matrix3<T>::operator *= ( const matrix3<T> &m ) {
    matrix3<T> *mr = new matrix3<T>( (T) 0 );
    mr->_00 = this->_00 * m._00 + this->_01 * m._10 + this->_02 * m._20;
    mr->_01 = this->_00 * m._01 + this->_01 * m._11 + this->_02 * m._21;
    mr->_02 = this->_00 * m._02 + this->_01 * m._12 + this->_02 * m._22;
    mr->_10 = this->_10 * m._00 + this->_11 * m._10 + this->_12 * m._20;
    mr->_11 = this->_10 * m._01 + this->_11 * m._11 + this->_12 * m._21;
    mr->_12 = this->_10 * m._02 + this->_11 * m._12 + this->_12 * m._22;
    mr->_20 = this->_20 * m._00 + this->_21 * m._10 + this->_22 * m._20;
    mr->_21 = this->_20 * m._01 + this->_21 * m._11 + this->_22 * m._21;
    mr->_22 = this->_20 * m._02 + this->_21 * m._12 + this->_22 * m._22;
    return *mr;
}


/**
 * @brief Transpõe a matriz 3x3
 */
template <class T>
void matrix3<T>::transpose() {
    matrix3<T> m(this->get_data());
    this->_01 = m._10;
    this->_02 = m._20;
    this->_10 = m._01;
    this->_12 = m._21;
    this->_20 = m._02;
    this->_21 = m._12;
}


/**
 * @brief Transpõe a matriz 3x3
 */
template <class T>
void matrix3<T>::transpose( matrix3<T> &m ) {
    /// Transpoẽ os dois elementos que serão modificados
    this->_01 = m._10;
    this->_02 = m._20;
    this->_10 = m._01;
    this->_12 = m._21;
    this->_20 = m._02;
    this->_21 = m._12;
}


/**
 * @brief Retorna o determinante da matriz 3x3
 *
 * @return O determinante da matriz
 */
template <class T>
T matrix3<T>::determinant() {
    return ((_00*_11*_22 + _10*_21*_02 + _01*_12*_20) - (_02*_11*_20 + _12*_21*_00 + _22*_01*_20));
}


/**
 * @brief Retorna o vetor com os dados da matriz 3x3
 *
 * @return Os dados da matriz
 */
template <class T>
T* matrix3<T>::get_data() {
    return this->_array;
}

/**
 * @brief Calcula a inversa desta matriz
 *
 * @return true se a matriz tiver inversa e falso caso contrário
 */
template <class T>
bool matrix3<T>::inverse() {
    matrix3<T> m(this->get_data());

    T det = this->determinant();

    if( det <= (T) 0 ) return false;

    T det_i = ((T)1) / det;

    this->_00 = ( m._22*m._11 - m._21*_12 ) / det_i;
    this->_01 = ( m._22*m._01 - m._21*_01 ) / det_i;
    this->_10 = ( m._12*m._01 - m._11*_01 ) / det_i;
    this->_11 = ( m._22*m._00 - m._20*_02 ) / det_i;
    this->_12 = ( m._12*m._00 - m._10*_01 ) / det_i;
    this->_20 = ( m._21*m._10 - m._20*_11 ) / det_i;
    this->_21 = ( m._21*m._00 - m._20*_01 ) / det_i;
    this->_22 = ( m._11*m._00 - m._10*_01 ) / det_i;

    return true;
}

/**
 * @brief Calcula a inversa de uma matriz
 *
 * @return true se a matriz tiver inversa e falso caso contra
 */
template <class T>
bool matrix3<T>::inverse( matrix3<T> &m ) {

    T det = this->determinant();

    if( det <= (T) 0 ) return false;

    T det_i = ((T)1) / det;

    this->_00 = ( _22*_11 - _21*_12 ) / det_i;
    this->_01 = ( _22*_01 - _21*_01 ) / det_i;
    this->_10 = ( _12*_01 - _11*_01 ) / det_i;
    this->_11 = ( _22*_00 - _20*_02 ) / det_i;
    this->_12 = ( _12*_00 - _10*_01 ) / det_i;
    this->_20 = ( _21*_10 - _20*_11 ) / det_i;
    this->_21 = ( _21*_00 - _20*_01 ) / det_i;
    this->_22 = ( _11*_00 - _10*_01 ) / det_i;

    return true;
}
